clc;
clear all;

%读取数据，能显示每帧数据
%%% This script is used to read the binary file produced by the DCA1000
%%% and Mmwave Studio
%%% Command to run in Matlab GUI -
%readDCA1000('<ADC capture bin file>') function [retVal] = readDCA1000(fileName)
%% global variables
% change based on sensor config
numADCSamples = 256; % number of ADC samples per chirp
numADCBits = 16; % number of ADC bits per sample
numRX = 4; % number of receivers
numLanes = 2; % do not change. number of lanes is always 2
isReal = 0; % set to 1 if real only data, 0 if complex data0
n_chirps=128;%每一帧中chirps数，no of chirp loops 
frame=1;%第多少帧

global c B K T Tc fs f0 lambda d
c=3.0e8;  
B=768e6;       %调频带宽
K=29.982e12;       %调频斜率
T=B/K;         %调频周期
Tc=160e-6;     %chirp总周期
fs=1e7;       %采样率
f0=77e9;       %初始频率
lambda=c/f0;   %雷达信号波长
d=lambda/2;    %天线阵列间距

N = 256;       %距离向FFT点数
M = 128;       %多普勒向FFT点数
Q = 180;       %角度FFT
max_range = c * fs / (2 * B); % 最大距离
max_speed = lambda / (4 * Tc); % 最大速度
%% read file
% read .bin file
fileName='D:\20241022\4.bin';
fid = fopen(fileName,'r');
adcData = fread(fid, 'int16');
% if 12 or 14 bits ADC per sample compensate for sign extension
if numADCBits ~= 16
    l_max = 2^(numADCBits-1)-1;
    adcData(adcData > l_max) = adcData(adcData > l_max) - 2^numADCBits;
end
fclose(fid);
fileSize = size(adcData, 1);
% real data reshape, filesize = numADCSamples*numChirps
if isReal
    numChirps = fileSize/numADCSamples/numRX;
    LVDS = zeros(1, fileSize);
    %create column for each chirp
    LVDS = reshape(adcData, numADCSamples*numRX, numChirps);
    %each row is data from one chirp
    LVDS = LVDS.';
else
    % for complex data
    % filesize = 2 * numADCSamples*numChirps
    numChirps = fileSize/2/numADCSamples/numRX;%总chirps数
    numframe=numChirps/n_chirps;%总帧数 no of frames，总chirps数除以每帧chirps数。
    LVDS = zeros(1, fileSize/2);
    %combine real and imaginary part into complex data
    %read in file: 2I is followed by 2Q
    counter = 1;
    for i=1:4:fileSize-1
        LVDS(1,counter) = adcData(i) + sqrt(-1)*adcData(i+2); 
        LVDS(1,counter+1) = adcData(i+1)+sqrt(-1)*adcData(i+3); 
        counter = counter + 2;
    end
        % create column for each chirp
        LVDS = reshape(LVDS, numADCSamples*numRX, numChirps);
        %each row is data from one chirp
        LVDS = LVDS.';
end
%organize data per RX
adcData = zeros(numRX,numChirps*numADCSamples);
for row = 1:numRX
    for i = 1: numChirps
        adcData(row, (i-1)*numADCSamples+1:i*numADCSamples) = LVDS(i, (row-1)*numADCSamples+1:row*numADCSamples);
    end
end
% return receiver data
adcData;
%radar_data = reshape(adcData,numADCSamples, numChirps, numframe, numRX);
data = reshape(adcData,numADCSamples,n_chirps,numRX,numframe);
data1 = data(:,:,1,1);
data2 = data(:,:,2,1);
data3 = data(:,:,3,1);
data4 = data(:,:,4,1);
% 对所有 Chirp 和接收通道执行距离 FFT
rangeFFT = fft(data, N, 1); % N 为距离 FFT 点数
% 对距离 FFT 结果进行多普勒 FFT
dopplerFFT = fft(rangeFFT, M, 2); % M 为速度 FFT 点数
% 对速度 FFT 结果进行角度 FFT
angleFFT = fft(dopplerFFT, Q, 3); % Q 为角度 FFT 点数
% 计算距离、速度和角度
range_bins = linspace(0, max_range, N); % 距离 bins
doppler_bins = linspace(-max_speed, max_speed, M); % 速度 bins
angle_bins = linspace(-90, 90, size(angleFFT, 3)); % 角度 bins
% 初始化元胞数组，按帧存储每帧的点云
pointCloud = cell(1, numframe);

for frameIdx = 1:numframe
    % 提取当前帧的 FFT 结果
    angleFFT_frame = angleFFT(:,:,:,frameIdx);
    
    % 设置幅度阈值，提取当前帧的强目标点
    threshold = 0.5 * max(abs(angleFFT_frame(:))); % 可以调节阈值
    strong_points = find(abs(angleFFT_frame) > threshold);

    % 初始化当前帧的点云
    framePointCloud = [];

    % 遍历当前帧中找到的强目标点
    for idx = 1:length(strong_points)
        [rangeIdx, dopplerIdx, angleIdx] = ind2sub(size(angleFFT_frame), strong_points(idx));
        
        % 检查 angleIdx 是否在有效范围内
        if angleIdx >= 1 && angleIdx <= length(angle_bins)
            angle_value = angle_bins(angleIdx);
        else
            continue; % 跳过超出范围的点
        end

        % 计算距离、速度和角度
        range_value = range_bins(rangeIdx);
        speed_value = doppler_bins(dopplerIdx);

        % 转换为笛卡尔坐标
        x = range_value * cosd(angle_value);
        y = range_value * sind(angle_value);
        z = speed_value; % 假设速度对应 z 轴

        % 添加到当前帧的点云
        framePointCloud = [framePointCloud; x, y, z];
    end
    
    % 将当前帧的点云保存到元胞数组中
    pointCloud{frameIdx} = framePointCloud;
end

% 可视化每一帧的点云（示例：仅展示第1帧的点云）
figure;
scatter3(pointCloud{1}(:,1), pointCloud{1}(:,2), pointCloud{1}(:,3), 'filled');
xlabel('X (meters)');
ylabel('Y (meters)');
zlabel('Speed (m/s)');
title('3D Point Cloud from Radar Echo - Frame 1');
grid on;


% for f= 1:100%numframe
%     [range(:,:,:,f),speed(:,:,:,f),angle(:,:,:,f)]  = threedfft(data(:,:,:,f), N, M, Q, numADCSamples,n_chirps,numRX);
% end
%plotRangeSpeed(dopplerFFT(:,:,1,:), N, M, fs, 1,numframe); 